/* version 3.52c. (c) Copyright 1993 by Joseph Felsenstein and Mary Kuhner.
   Written by Mary Kuhner, Jon Yamato, Joseph Felsenstein, Akiko Fuseki,
   Sean Lamont, and Andrew Keeffe.
   Permission is granted to copy and use this program provided no fee is
   charged for it and provided that this copyright notice is not removed. */

#include <vector>
#include <string>
#include <fstream>
#include <sstream>
#include <iostream>
using std::vector;
using std::string;
using std::cerr;
using std::ostringstream;
#include "phylip_nearest_neighbor.h"

namespace phylip_tree
{
void dump(const string& description, const vector<pnode>& nodes)
{
	cerr << description << "\n";
	for (unsigned i = 0; i < nodes.size(); ++i)
	{
		cerr << "[" << i << "]"
			 << "\tnayme = "  << nodes[i]->name	
			 << "\tlength = " << nodes[i]->length		
			 << "\tnumber = " << nodes[i]->number << "\n";
	}
	cerr << description << "\n\n";
	
}

std::ostream& dump(pnode node, std::ostream& os)
{
	os << "\tnayme = "		<< node->name	
		 << "\tlength = "	<< node->length		
		 << "\thex = "		<< (unsigned)(node)
		 << "\tnumber = "	<< node->number << "\n";
	return os;
	
}




void phylip_prepare_empty_tree(t_phylip_tree& tree, const vector<string>& names)
{
	assert(names.size() == tree.cnt_leaves);
	/* initialize a t_phylip_tree */

	unsigned cnt_leaves2 = tree.cnt_leaves * 2 - 1;
	tree.nodep.assign(cnt_leaves2, 0);

	unsigned vec_node_pos = 0;

	// leaf nodes
	for (unsigned i = 0; i < tree.cnt_leaves; ++i)
	{
		tree.nodep[i] = &tree.vec_node[vec_node_pos++];
		tree.nodep[i]->is_leaf = true;
		tree.nodep[i]->number = i + 1;
		tree.nodep[i]->name = names[i];
		tree.nodep[i]->length = 0.0;
	}
	
	for (unsigned i = tree.cnt_leaves + 1; i < cnt_leaves2; ++i)
	{
		t_node * q = 0;
		t_node * p = 0;
		for (unsigned j = 1; j <= 3; ++j)
		{
			p = &tree.vec_node[vec_node_pos++];
			p->is_leaf = false;
			p->number = i;
			p->next = q;
			q = p;
		}
		p->next->next->next = p;
		tree.nodep[i - 1] = p;
	}
	t_node * q = 0;
	t_node * p = 0;
	for (unsigned j = 1; j <= 2; ++j)
	{
		p = &tree.vec_node[vec_node_pos++];
		p->is_leaf = false;
		p->number = cnt_leaves2;
		p->next = q;
		q = p;
	}
	p->next->next = p;
	tree.nodep[cnt_leaves2 - 1] = p;
	tree.start = tree.nodep[0];
	assert(tree.vec_node.size () == vec_node_pos);
}  /* phylip_prepare_empty_tree */


inline void hookup(t_node *p, t_node *q)
{
	/* hook together two nodes */
	p->back = q;
	q->back = p;
}  /* hookup */




//----------------------------------------------------------------------------------------

//	phylip_join_up_tree

//----------------------------------------------------------------------------------------
void phylip_join_up_tree(t_phylip_tree& tree, t_matrix& matrix,
								t_progress_indicator& dots)
{		
	assert(tree.cnt_leaves);
	if (!tree.cnt_leaves)
		return;
	assert(tree.cnt_leaves == matrix.size());
	// prepare a list of clusters
	std::vector<pnode> cluster(tree.cnt_leaves);
	for (unsigned i = 0; i < tree.cnt_leaves; ++i)
		cluster[i] = tree.nodep[i];
	
	unsigned min_i = 0;
	unsigned min_j = 0;

	// is this necessary in a symetrical matrix?
	for (unsigned i = 0; i < tree.cnt_leaves - 1 ; ++i)
		for (unsigned j = i + 1; j < tree.cnt_leaves; ++j)
			assert(matrix[j][i] == matrix[i][j]);
//			matrix[j][i] =
//			matrix[i][j] = (matrix[j][i] + matrix[i][j]) / 2.0;
	
	
		
	double fotu2 = tree.cnt_leaves - 2.0;
	unsigned nextnode = tree.cnt_leaves + 1;
	
	vector<double>	avg(tree.cnt_leaves, 0.0);
	vector<long>	cluster_size(tree.cnt_leaves, 1);
	
	// Enter the main cycle
	for (unsigned nc = 3; nc < tree.cnt_leaves; ++nc)			
	{
		++dots;
		for (unsigned j = 1; j < tree.cnt_leaves ; ++j)
		{
			for (unsigned i = 0; i < j; ++i)
				matrix[i][j] = matrix[j][i];
		}
		double tmin = 99999.0;
		
		
		// Compute sij and minimize
		for (unsigned ja = 2; ja < tree.cnt_leaves + 1; ++ja)
		{
			if (cluster[ja - 1])
			{
				for (unsigned ia = 0; ia + 1 < ja; ++ia)
				{
					if (cluster[ia])
					{
						double diq = 0.0;
						double djq = 0.0;
						double dij = matrix[ja - 1][ia];
						for (unsigned i = 0; i < tree.cnt_leaves; ++i)
						{
							diq += matrix[ia][i];
							djq += matrix[ja - 1][i];
						}
						
						double total = fotu2 * dij - diq - djq;
						if (total < tmin)
						{
							tmin = total;
							min_i = ia + 1;
							min_j = ja;
						}
					}
				}
			}
		}
		// compute lengths and print
		double dio = 0.0;
		double djo = 0.0;
		for (unsigned i = 0; i < tree.cnt_leaves; ++i)
		{
			dio += matrix[min_i - 1][i];
			djo += matrix[min_j - 1][i];
		}
		double dmin = matrix[min_j - 1][min_i - 1];
		double bi = (dmin + (dio - djo) / fotu2) * 0.5;
		double bj = dmin - bi;
		bi -= avg[min_i - 1];
		bj -= avg[min_j - 1];
		hookup(tree.nodep[nextnode - 1]->next, cluster[min_i - 1]);
		hookup(tree.nodep[nextnode - 1]->next->next, cluster[min_j - 1]);
		cluster[min_i - 1]->length = bi;
		cluster[min_j - 1]->length = bj;
		cluster[min_i - 1]->back->length = bi;
		cluster[min_j - 1]->back->length = bj;
		cluster[min_i - 1] = tree.nodep[nextnode - 1];
		cluster[min_j - 1] = 0;
		++nextnode;
		avg[min_i - 1] = dmin * 0.5;
		// re-initialization
		fotu2 -= 1.0;
		for (unsigned j = 0; j < tree.cnt_leaves; ++j)
		{
			if (cluster[j])
			{
				double da = (matrix[j][min_i - 1] + matrix[j][min_j - 1]) * 0.5;
				if (min_i < j + 1)
					matrix[j][min_i - 1] = da;
				if (min_i > j + 1)
					matrix[min_i - 1][j] = da;
			}
		}
		for (unsigned j = 0; j < tree.cnt_leaves; ++j)
			matrix [j		][min_j - 1]	=
			matrix [min_j - 1][j		  ] = 0.0;
		
		cluster_size[min_i - 1] += cluster_size[min_j - 1];
	}
	// the last cycle
	unsigned nude = 1;
	unsigned el[3];
	for (unsigned i = 0; i < tree.cnt_leaves; ++i)
	{
		if (cluster[i])
		{
			el[nude - 1] = i;
			++nude;
		}
	}
	double bi = (matrix[el[1]][el[0]] +
				 matrix[el[2]][el[0]] -
				 matrix[el[2]][el[1]]) * 0.5;
	double bj = matrix[el[1]][el[0]] - bi;
	double bk = matrix[el[2]][el[0]] - bi;
	bi -= avg[el[0]];
	bj -= avg[el[1]];
	bk -= avg[el[2]];
	hookup(tree.nodep[nextnode - 1], cluster[el[0]]);
	hookup(tree.nodep[nextnode - 1]->next, cluster[el[1]]);
	hookup(tree.nodep[nextnode - 1]->next->next, cluster[el[2]]);
	cluster[el[0]]->length = bi;
	cluster[el[1]]->length = bj;
	cluster[el[2]]->length = bk;
	cluster[el[0]]->back->length = bi;
	cluster[el[1]]->back->length = bj;
	cluster[el[2]]->back->length = bk;
	tree.start = cluster[el[0]]->back;
}

//----------------------------------------------------------------------------------------

//	phylip_join_up_tree

//----------------------------------------------------------------------------------------
void phylip_join_up_tree_upgma(t_phylip_tree& tree, t_matrix& matrix,
								t_progress_indicator& dots)
{		
	assert(tree.cnt_leaves);
	if (!tree.cnt_leaves)
		return;
	
	tree.UPGMA = true;
	assert(tree.cnt_leaves == matrix.size());
	// prepare a list of clusters
	std::vector<pnode> cluster(tree.cnt_leaves);
	for (unsigned i = 0; i < tree.cnt_leaves; ++i)
		cluster[i] = tree.nodep[i];
	
#ifndef NDEBUG
	for (unsigned i = 0; i < tree.cnt_leaves - 1 ; ++i)
		for (unsigned j = i + 1; j < tree.cnt_leaves; ++j)
			assert(matrix[j][i] == matrix[i][j]);
#endif
	
	/*for (unsigned i = 0; i < tree.cnt_leaves - 1 ; ++i)
	{
		for (unsigned j = i + 1; j < tree.cnt_leaves; ++j)
		{
			matrix[j][i] =
			matrix[i][j] = (matrix[j][i] + matrix[i][j]) / 2.0;
		}
	}
	*/
	double fotu2 = tree.cnt_leaves - 2.0;
	unsigned nextnode = tree.cnt_leaves;
	
	vector<double> avg(tree.cnt_leaves, 0.0);
	vector<long> cluster_size(tree.cnt_leaves, 1);
	
	// Enter the main cycle
	for (unsigned nc = 1; nc < tree.cnt_leaves; ++nc)	// UPGMA
	{
		++dots;
		for (unsigned j = 1; j < tree.cnt_leaves; ++j)
			for (unsigned i = 0; i < j; ++i)
				matrix[i][j] = matrix[j][i];
		

		// Find smallest value in matrix
		unsigned min_i;
		unsigned min_j;
		double tmin = 99999.0;
		for (unsigned ja = 1; ja < tree.cnt_leaves; ++ja)
		{
			if (!cluster[ja])
				continue;
			for (unsigned ia = 0; ia < ja; ++ia)
			{
				if (!cluster[ia])
					continue;
				double total = matrix[ja][ia];
				
				if (total < tmin)
				{
					tmin = total;
					min_i = ia;
					min_j = ja;
				}
			}
		}
		
		// compute lengths and print
		double bi = - avg[min_i];
		if (matrix[min_j][min_i] < 5000.0)		//new
			bi += matrix[min_j][min_i] / 2.0;
		double bj = - avg[min_j];
		if (matrix[min_j][min_i] < 5000.0)		//new
			bj += matrix[min_j][min_i] / 2.0;
		avg[min_i] += bi;

		hookup(tree.nodep[nextnode]->next, cluster[min_i]);
		hookup(tree.nodep[nextnode]->next->next, cluster[min_j]);
		cluster[min_i]->length = bi;
		cluster[min_j]->length = bj;
		cluster[min_i]->back->length = bi;
		cluster[min_j]->back->length = bj;
		cluster[min_i] = tree.nodep[nextnode];
		cluster[min_j] = 0;
		++nextnode;
		// re-initialization
		fotu2 -= 1.0;
		for (unsigned j = 0; j < tree.cnt_leaves; ++j)
		{
//			if (cluster[j])				// old
			if (cluster[j] && matrix[j][min_i] < 5000.0 && matrix[j][min_j] < 5000.0)
			{
				matrix[j][min_i] =
				matrix[min_i][j] = (matrix[j][min_i] * cluster_size[min_i] +
									   matrix[j][min_j] * cluster_size[min_j]	)
									  /
									  (cluster_size[min_i] + cluster_size[min_j]);
			}
		}
		for (unsigned j = 0; j < tree.cnt_leaves; ++j)
		{
			matrix[j][min_j] = 0.0;
			matrix[min_j][j] = 0.0;
		}
		cluster_size[min_i] += cluster_size[min_j];
	}
	// the last cycle
	unsigned el;
	for (unsigned i = 0; i < tree.cnt_leaves; ++i)
		if (cluster[i])
		{
			tree.start = cluster[i];
			break;
		}
	
}

//________________________________________________________________________________________

//		phylip_make_tree

//________________________________________________________________________________________
void phylip_make_tree(t_phylip_tree& cur_tree, const vector<string>& names,
						t_matrix& matrix)
{
	ostringstream ignore_strm;
	t_progress_indicator dots(ignore_strm, 10000);
	phylip_make_tree(cur_tree, names, matrix, dots);
}

void phylip_make_tree(t_phylip_tree& cur_tree, const vector<string>& names,
						t_matrix& matrix, t_progress_indicator& dots)
{
	// construct tree nodes
	assert(names.size() == matrix.size());
	assert(cur_tree.cnt_leaves == matrix.size());
	phylip_prepare_empty_tree(cur_tree, names);
	phylip_join_up_tree_upgma(cur_tree, matrix, dots);
	
}








template<typename TREEMAKER> void phylip_treeout(	TREEMAKER& maker,
													const t_phylip_tree& cur_tree,
													t_node *p)
{
	assert(p);
	if (p->is_leaf)
	{
		maker.add_leaf(p->name);
		cerr << "add_leaf " << p->back<< "\n";
	}
	else
	{
		cerr << "add_internal_node\n";
		maker.add_internal_node();
		phylip_treeout(maker, cur_tree, p->next->back);
		cerr << "add_sibling\n";
		maker.add_sibling();
		phylip_treeout(maker, cur_tree, p->next->next->back);
		if (p == cur_tree.start && !cur_tree.UPGMA)
		{
			cerr << "add_sibling\n";
			maker.add_sibling();
			phylip_treeout(maker, cur_tree, p->back);
		}
		cerr << "to_grandparent\n";
		maker.reparent_to_grandparent();
	}
	if (p == cur_tree.start)
	{
		cerr << "finish\n";
		maker.finish();
	}
	else
	{
		cerr << "to_grandparent\n";
		maker.set_branch_len(p->length);
	}

}


//8888888888888888888888888888888888888888888888888888888888888888888888888888888888888888

//		str_tree_maker

//________________________________________________________________________________________
//8888888888888888888888888888888888888888888888888888888888888888888888888888888888888888
struct str_tree_maker
{
	string str;
	void add_leaf(const string& name);
	void add_internal_node();
	void set_branch_len(double len);
	void reparent_to_grandparent();
	void finish();
	void add_sibling();
};
void str_tree_maker::add_leaf(const string& name)
{
//	cerr << "Add leaf " << name << "\n";
	str += name;
}
void str_tree_maker::add_internal_node()
{
//	cerr << "Add internal node \n";
	str += '(';
}
void str_tree_maker::set_branch_len(double len)
{
//	cerr << "set branch len " << len << "\n";
	ostringstream os;
	os << len;
	str += ':' + os.str();
}
void str_tree_maker::reparent_to_grandparent()
{
//	cerr << "Reparent\n";
	str += ')';
}
void str_tree_maker::finish()
{
//	cerr << "Finish\n";
	str += ';';
}
void str_tree_maker::add_sibling()
{
	str += ',';
}

string phylip_tree_to_str(t_phylip_tree& cur_tree)
{
	str_tree_maker phylip_tree_str;
	phylip_treeout(phylip_tree_str, cur_tree, cur_tree.start);
	return phylip_tree_str.str;
}
//8888888888888888888888888888888888888888888888888888888888888888888888888888888888888888



}

